#include "body.h"
#include "arttracking.h"

namespace Tracking{

	#pragma region BodyBase
	BodyBase::BodyBase(ARTTracking::BodyType type,int id)
	{
		_id = id;
		_bodytype = type;
		_transformationMatrix = mat4f::IDENTITY;

		_quality = -1.0;
		
	}

	BodyBase::~BodyBase()
	{
	
	}

	TrackingState BodyBase::getTrackingState()
	{
		if(_quality >0)
			return TrackingState::TRACKED;
		else
			return TrackingState::UNTRACKED;
	}

	void BodyBase::_update(const DTrack_Body_Type_d &body)
	{
		mat4f t = mat4f::IDENTITY;

		//set translation (scale to meters)
		vec3f pos(body.loc[0]/1000,body.loc[1]/1000,body.loc[2]/1000);
		t.set_translation( pos );

		//set rotation
		t.array[0] = body.rot[0]; t.array[4] = body.rot[3]; t.array[8] = body.rot[6];
		t.array[1] = body.rot[1]; t.array[5] = body.rot[4]; t.array[9] = body.rot[7];
		t.array[2] = body.rot[2]; t.array[6] = body.rot[5]; t.array[10] = body.rot[8];

		//update Body
		_transformationMatrix = t;
		_quality = body.quality;
	}

	mat4f BodyBase::getTransformationMatrix()
	{
		if(getTrackingState()==TrackingState::TRACKED)
			return _transformationMatrix;
		else
			return mat4f::IDENTITY;
	}

	#pragma endregion BodyBase

	#pragma region Body
	Body::Body(int id) : BodyBase(ARTTracking::BodyType::BODY,id)
	{
		_transformationMatrix = mat4f::IDENTITY;
		_quality = -1.0f;
	}

	void Body::_update(const DTrack_Body_Type_d &body)
	{
		BodyBase::_update(body);
	}


	#pragma endregion Body

	#pragma region FlyStick
	FlyStick::FlyStick(int id) : BodyBase(ARTTracking::BodyType::FLYSTICK,id)
	{
		_triggerMatrix = mat4f::IDENTITY;
		_deltaMatrix = mat4f::IDENTITY;
		for(int i=0;i<5;i++)
			_buttons[i] = ButtonState::UNPRESSED;

		_joystickValue[0]=_joystickValue[1]=0.0f;
	}

	void FlyStick::_update(const DTrack_FlyStick_Type_d &flystick)
	{
		DTrack_Body_Type_d temp; 

		temp.quality = flystick.quality;
		temp.id=flystick.id;
		temp.rot[0]=flystick.rot[0];temp.rot[1]=flystick.rot[1];temp.rot[2]=flystick.rot[2];
		temp.rot[3]=flystick.rot[3];temp.rot[4]=flystick.rot[4];temp.rot[5]=flystick.rot[5];
		temp.rot[6]=flystick.rot[6];temp.rot[7]=flystick.rot[7];temp.rot[8]=flystick.rot[8];
		temp.loc[0] = flystick.loc[0]; temp.loc[1] = flystick.loc[1]; temp.loc[2] = flystick.loc[2];

		//update body
		BodyBase::_update(temp);

		//update joystick
		_joystickValue[0] = flystick.joystick[0];_joystickValue[1] = flystick.joystick[1];

		//update buttons
		_handleButtonState(const_cast<int*>(flystick.button));

		//store trigger matrix if trigger is pushed
		if(getButtonState(Button::TRIGGER)==ButtonState::TRIGGERED)
			_triggerMatrix = _transformationMatrix;
		if(getButtonState(Button::TRIGGER)==ButtonState::RELEASED)
			_triggerMatrix  = mat4f::IDENTITY;	
	}	

	void FlyStick::_handleButtonState(int* b)
	{
	
		for(int i=0;i<5;i++){
			if(_buttons[i] == ButtonState::UNPRESSED){
				if(b[i] == 0)
					break;
				else
					_buttons[i]=ButtonState::TRIGGERED;
				break;
			}

			if(_buttons[i] == ButtonState::TRIGGERED){
				if(b[i] == 0)
					_buttons[i]=ButtonState::UNPRESSED;
				else
					_buttons[i]=ButtonState::PRESSED;
				break;
			}

			if(_buttons[i] == ButtonState::PRESSED){
				if(b[i] == 0)
					_buttons[i]=ButtonState::RELEASED;
				else
					_buttons[i]=ButtonState::PRESSED;
				break;
			}

			if(_buttons[i] == ButtonState::RELEASED){
				if(b[i] == 0)
					_buttons[i]=ButtonState::UNPRESSED;
				else
					_buttons[i]=ButtonState::PRESSED;
				break;
			}
		}

	
	}

	const mat4f& FlyStick::getDeltaMatrix()
	{


		mat4f delta = mat4f::IDENTITY;

		if(getButtonState(Button::TRIGGER) != ButtonState::PRESSED)
			_deltaMatrix = mat4f::IDENTITY;
		else{
		
			mat4f triggered_trans = mat4f::IDENTITY;
			_triggerMatrix.transpose_to(triggered_trans);

			delta = triggered_trans * _transformationMatrix;
	
			delta.array[3]=delta.array[7]=delta.array[11]=delta.array[12]=delta.array[13]=delta.array[14]=0;
			delta.array[15]=1;

			mat4f delta2;
			delta.transpose_to(delta2);
		
			delta2.x() = _transformationMatrix.x() - _triggerMatrix.x();
			delta2.y() = _transformationMatrix.y() - _triggerMatrix.y();
			delta2.z() = _transformationMatrix.z() - _triggerMatrix.z();
		
			_deltaMatrix = delta2;
			}
		//std::cout<<_deltaMatrix<<std::endl;

		return _deltaMatrix;;


	}
	
		const mat4f& FlyStick::getTest()
		{
					mat4f ret = mat4f::IDENTITY;
			return ret;
		}
	#pragma endregion FlyStick

}